using System.Collections.Generic;
using UnityEngine;

namespace Gj.Pathfinding
{
    internal class JPSPlusNode : JPSNode
    {
        public JPSPlusNode(int x, int y) : this(new Vector2Int(x, y))
        {
        }

        public JPSPlusNode(Vector2Int l) : base(l)
        {
            Dist = new int[8];
            for (var i = 0; i < 8; i++) Dist[i] = int.MinValue;
            IsJumpPoint = false;
        }

        public float GivenCost { get; set; }
        public float FinalCost { get; set; }
        public int[] Dist { get; }
        public bool IsJumpPoint { get; set; }

        public new JPSPlusNode Parent { get; set; }

        public new void Revert()
        {
            base.Revert();
            GivenCost = 0;
            FinalCost = 0;
        }
    }

    internal class JPSPlusGrid : JPSGrid
    {
        internal static Vector2Int[] Directions = new Vector2Int[8]
        {
            Vector2Int.up,
            Vector2Int.right,
            Vector2Int.down,
            Vector2Int.left,
            new(1, 1),
            new(1, -1),
            new(-1, -1),
            new(-1, 1)
        };

        private static readonly int[] OppositeDirIdx = new int[8]
        {
            2, 3, 0, 1,
            6, 7, 4, 5
        };

        protected new readonly JPSPlusNode[,] _grid;

        public JPSPlusGrid(int[,] navigable) : base(navigable)
        {
            _grid = new JPSPlusNode[BoundsMaxX + 1, BoundsMaxY + 1];
            for (var x = BoundsMinX; x <= BoundsMaxX; x++)
            for (var y = BoundsMinY; y <= BoundsMaxY; y++)
                _grid[x, y] = new JPSPlusNode(x, y);
            CalculatePrimaryJumpPoint();
            CalculateJumpPointDist();
            CaluclateDist_Wall();
        }

        internal new JPSPlusNode this[int x, int y] => _grid[x, y];
        internal new JPSPlusNode this[Vector2Int location] => _grid[location.x, location.y];

        /// <summary>
        ///     提取基础跳点
        /// </summary>
        private void CalculatePrimaryJumpPoint()
        {
            for (var x = BoundsMinX; x <= BoundsMaxX; x++)
            for (var y = BoundsMinY; y <= BoundsMaxY; y++)
            {
                if (!_N(x, y, 1)) continue;
                var g = _grid[x, y];
                bool n = _N(x, y + 1, 1),
                    s = _N(x, y - 1, 1),
                    e = _N(x + 1, y, 1),
                    w = _N(x - 1, y, 1),
                    ne = !_N(x + 1, y + 1, 1),
                    nw = !_N(x - 1, y + 1, 1),
                    se = !_N(x + 1, y - 1, 1),
                    sw = !_N(x - 1, y - 1, 1);
                if (n && e && ne)
                    g.IsJumpPoint = true;
                if (n && w && nw)
                    g.IsJumpPoint = true;
                if (s && w && sw)
                    g.IsJumpPoint = true;
                if (s && e && se)
                    g.IsJumpPoint = true;
            }
        }

        #region test

        public List<Vector2Int> GetJumpPoints()
        {
            var res = new List<Vector2Int>();
            for (var x = BoundsMinX; x <= BoundsMaxX; x++)
            for (var y = BoundsMinY; y <= BoundsMaxY; y++)
                if (_navigable[x, y] != 1)
                    //if (this[x, y].IsJumpPoint)
                    res.Add(new Vector2Int(x, y));
            return res;
        }

        #endregion

        #region 跳点距离计算

        /// <summary>
        ///     计算跳点距离
        /// </summary>
        private void CalculateJumpPointDist()
        {
            var cardinal = new List<Vector2Int>();
            for (var x = BoundsMinX; x <= BoundsMaxX; x++)
            for (var y = BoundsMinY; y <= BoundsMaxY; y++)
                if (this[x, y].IsJumpPoint)
                    for (var i = 0; i < 4; i++)
                        SpreadSingleJumpPoint(new Vector2Int(x, y) + Directions[i], i, 1, true, ref cardinal);
            foreach (var vec in cardinal)
                if (_N(vec.x, vec.y, 1))
                    this[vec.x, vec.y].IsJumpPoint = true;
            for (var x = BoundsMinX; x <= BoundsMaxX; x++)
            for (var y = BoundsMinY; y <= BoundsMaxY; y++)
                if (this[x, y].IsJumpPoint)
                    for (var i = 4; i < 8; i++)
                        SpreadSingleJumpPoint(new Vector2Int(x, y) + Directions[i], i, 1, false, ref cardinal);
        }

        private void SpreadSingleJumpPoint(Vector2Int vec, int dirIdx, int dist, bool isCardinal,
            ref List<Vector2Int> cardinalList)
        {
            // 遇到障碍即刻终止
            if (!_N(vec.x, vec.y, 1)) return;
            var g = _grid[vec.x, vec.y];
            if (isCardinal) cardinalList.Add(vec);
            // 把蔓延方向翻转一下，指向其来源
            var idx = OppositeDirIdx[dirIdx];
            // 选择最近的跳点作为来源
            if (g.Dist[idx] == int.MinValue || g.Dist[idx] > dist)
                g.Dist[idx] = dist;
            // 递归计算
            SpreadSingleJumpPoint(vec + Directions[dirIdx], dirIdx, dist + 1, isCardinal, ref cardinalList);
        }

        private void SpreadSingleJumpPoint(Vector2Int vec, int dirIdx, int dist)
        {
            // 遇到障碍即刻终止
            if (!_N(vec.x, vec.y, 1)) return;
            var g = _grid[vec.x, vec.y];
            // 把蔓延方向翻转一下，指向其来源
            var idx = OppositeDirIdx[dirIdx];
            // 选择最近的跳点作为来源
            if (g.Dist[idx] == int.MinValue || g.Dist[idx] > dist)
                g.Dist[idx] = dist;
            // 递归计算
            SpreadSingleJumpPoint(vec + Directions[dirIdx], dirIdx, dist + 1);
        }

        #endregion

        #region 障碍物距离计算

        private void CaluclateDist_Wall()
        {
            // 统计墙壁
            for (var x = BoundsMinX - 1; x <= BoundsMaxX + 1; x++)
            {
                SpreadCalculateObstacle(x, BoundsMinY - 1);
                SpreadCalculateObstacle(x, BoundsMaxY + 1);
            }

            for (var y = BoundsMinY - 1; y <= BoundsMaxY + 1; y++)
            {
                SpreadCalculateObstacle(BoundsMinX - 1, y);
                SpreadCalculateObstacle(BoundsMaxX + 1, y);
            }

            // 统计障碍物
            for (var x = BoundsMinX; x <= BoundsMaxX; x++)
            for (var y = BoundsMinY; y <= BoundsMaxY; y++)
                if (_navigable[x, y] != 1)
                    SpreadCalculateObstacle(x, y);
        }

        private void SpreadCalculateObstacle(int x, int y)
        {
            for (var i = 0; i < 8; i++)
            {
                var spreadDir = Directions[i];
                SpreadCalculateSingleObstacle(new Vector2Int(x, y) + spreadDir, i, 0);
            }
        }

        private void SpreadCalculateSingleObstacle(Vector2Int vec, int dirIdx, int dist)
        {
            // 遇到障碍即刻终止
            if (!_N(vec.x, vec.y, 1)) return;
            var g = _grid[vec.x, vec.y];
            // 把蔓延方向翻转一下，指向其来源
            var idx = OppositeDirIdx[dirIdx];
            // 选择最近的障碍物作为来源
            if (g.Dist[idx] == int.MinValue || g.Dist[idx] < dist)
                g.Dist[idx] = dist;
            // 递归计算
            SpreadCalculateSingleObstacle(vec + Directions[dirIdx], dirIdx, dist - 1);
        }

        #endregion
    }
    //
    // internal class JPSPlus
    // {
    //     private static Dictionary<JPSDir, JPSDir[]> ValidDirLookUpTable = new Dictionary<JPSDir, JPSDir[]>
    //     {
    //         { JPSDir.S,      new []{ JPSDir.W,  JPSDir.SW, JPSDir.S, JPSDir.SE, JPSDir.E } },
    //         { JPSDir.SE, new []{ JPSDir.S, JPSDir.SE, JPSDir.E } },
    //         { JPSDir.E,       new []{ JPSDir.S, JPSDir.SE, JPSDir.E, JPSDir.NE, JPSDir.N } },
    //         { JPSDir.NE, new []{ JPSDir.E,  JPSDir.NE, JPSDir.N } },
    //         { JPSDir.N,      new []{ JPSDir.E,  JPSDir.NE, JPSDir.N, JPSDir.NW, JPSDir.W } },
    //         { JPSDir.NW, new []{ JPSDir.N, JPSDir.NW, JPSDir.W } },
    //         { JPSDir.W,       new []{ JPSDir.N, JPSDir.NW, JPSDir.W, JPSDir.SW, JPSDir.S } },
    //         { JPSDir.SW, new []{ JPSDir.W,  JPSDir.SW, JPSDir.S } }
    //     };
    //     private JPSDir[] allDirections = Enum.GetValues(typeof(JPSDir)).Cast<JPSDir>().ToArray();
    //
    //     private static readonly Vector2Int NullVec = new Vector2Int(-1, -1);
    //     private readonly JPSPlusGrid _grid;
    //     private Vector2Int _start = NullVec, _goal = NullVec;
    //     private readonly FastPriorityQueue<JPSPlusNode> _open;
    //     
    //     public JPSPlus(JPSPlusGrid grid, int cap = 256)
    //     {
    //         _grid = grid;
    //         _open = new FastPriorityQueue<JPSPlusNode>(cap);
    //     }
    //
    //     public List<Vector2Int> FindPath(Vector2Int start, Vector2Int goal)
    //     {
    //         if (!_grid._N(start.x, start.y) || !_grid._N(goal.x, goal.y))
    //             return null;
    //         _grid.Clear();
    //         _start = start;
    //         _goal = goal;
    //         _open.Clear();
    //         var startNode = _grid[start];
    //         _open.Enqueue(startNode, startNode.GivenCost);
    //         while (_open.Count != 0)
    //         {
    //             JPSPlusNode current = _open.Dequeue();
    //             current.Closed = true;
    //             if (current.Location == goal)
    //                 return Trace(current);
    //             IdentitySuccessors(current);
    //         }
    //         return null;
    //     }
    //
    //     private void IdentitySuccessors(JPSPlusNode current)
    //     {
    //         foreach (var direction in GetValidDir(current))
    //         {
    //             JPSPlusNode newSuccessor = null;
    //             float givenCost = 0;
    //             // 水平方向，立刻到达
    //             if (IsCardinal(direction) &&
    //                 IsGoalInThisDirExactly(_goal, current.Location, direction) &&
    //                 DiffNode(_goal, current.Location) <= Mathf.Abs(current.Dist[(int)direction])
    //                 )
    //             {
    //                 newSuccessor = _grid[_goal];
    //                 givenCost = current.GivenCost + DiffNode(_goal, current.Location);
    //             }
    //             else
    //             // 对角线上优先对齐到水平方向
    //             if (IsDiagonal(direction) &&
    //                 IsGoalInThisDirGenerally(_goal, current.Location, direction) &&
    //                 (DiffNode_x(_goal, current.Location) <= Mathf.Abs(current.Dist[(int)direction]) ||
    //                 DiffNode_y(_goal, current.Location) <= Mathf.Abs(current.Dist[(int)direction])))
    //             {
    //                 int min_diff = Mathf.Min(Mathf.Abs(_goal.x - current.Location.x),
    //                                          Mathf.Abs(_goal.y - current.Location.y));
    //                 newSuccessor = _grid[current.Location + JPSPlusGrid.Directions[(int)direction] * min_diff];
    //             }
    //             else
    //             // 能走就走
    //             if (current.Dist[(int)direction] > 0)
    //             {
    //                 newSuccessor = _grid[current.Location + JPSPlusGrid.Directions[(int)direction] * current.Dist[(int)direction]];
    //                 givenCost = DiffNode(newSuccessor.Location, current.Location);
    //                 if (IsDiagonal(direction))
    //                 {
    //                     givenCost = givenCost * JPSPlusGrid.SQRT2;
    //                 }
    //                 givenCost += current.GivenCost;
    //             }
    //             // 传统A*
    //             if (newSuccessor != null)
    //             {
    //                 if (newSuccessor.Closed)
    //                     continue;
    //                 if (!newSuccessor.Opened)
    //                 {
    //                     newSuccessor.Parent = current;
    //                     newSuccessor.GivenCost = givenCost;
    //                     newSuccessor.dirFromParent = direction;
    //                     newSuccessor.FinalCost = givenCost + Heuristic(newSuccessor.Location, _goal);
    //                     newSuccessor.Opened = true;
    //                     _open.Enqueue(newSuccessor, newSuccessor.FinalCost);
    //                 }
    //                 else if (givenCost < newSuccessor.GivenCost)
    //                 {
    //                     newSuccessor.Parent = current;
    //                     newSuccessor.GivenCost = givenCost;
    //                     newSuccessor.dirFromParent = direction;
    //                     newSuccessor.FinalCost = givenCost + Heuristic(newSuccessor.Location, _goal);
    //                     newSuccessor.Opened = true;
    //                     _open.UpdatePriority(newSuccessor, newSuccessor.FinalCost);
    //                 }
    //             }
    //         }
    //     }
    //
    //
    //     private JPSDir[] GetValidDir(JPSPlusNode current)
    //     {
    //         return current.Parent == null ? allDirections : ValidDirLookUpTable[current.dirFromParent];
    //     }
    //
    //     // 判断方向的类型
    //     private bool IsCardinal(JPSDir dir)
    //     {
    //         return dir <= JPSDir.W;
    //     }
    //     private bool IsDiagonal(JPSDir dir)
    //     {
    //         return dir >= JPSDir.NE;
    //     }
    //     
    //     /// 目标是否大致在这个方向
    //     private bool IsGoalInThisDirGenerally(Vector2Int goal, Vector2Int current, JPSDir dir)
    //     {
    //         Vector2Int delta = goal - current;
    //         switch (dir)
    //         {
    //             case JPSDir.E:
    //                 return delta.x > 0 && delta.y == 0;
    //             case JPSDir.N:
    //                 return delta.y > 0 && delta.x == 0;
    //             case JPSDir.NE:
    //                 return delta.x > 0 && delta.y > 0;
    //             case JPSDir.W:
    //                 return delta.x < 0 && delta.y == 0;
    //             case JPSDir.NW:
    //                 return delta.x < 0 && delta.y > 0;
    //             case JPSDir.S:
    //                 return delta.y < 0 && delta.x == 0;
    //             case JPSDir.SE:
    //                 return delta.y < 0 && delta.x > 0;
    //             case JPSDir.SW:
    //                 return delta.y < 0 && delta.x < 0;
    //         }
    //         return false;
    //     }
    //     /// 目标是否严格在这个方向上
    //     private bool IsGoalInThisDirExactly(Vector2Int goal, Vector2Int current, JPSDir dir)
    //     {
    //         Vector2Int delta = goal - current;
    //         switch(dir)
    //         {
    //             case JPSDir.E:
    //                 return delta.x > 0 && delta.y == 0;
    //             case JPSDir.N:
    //                 return delta.y > 0 && delta.x == 0;
    //             case JPSDir.NE:
    //                 return delta.x > 0 && delta.y > 0 && Mathf.Abs(delta.x) == Mathf.Abs(delta.y);
    //             case JPSDir.W:
    //                 return delta.x < 0 && delta.y == 0;
    //             case JPSDir.NW:
    //                 return delta.x < 0 && delta.y > 0 && Mathf.Abs(delta.x) == Mathf.Abs(delta.y);
    //             case JPSDir.S:
    //                 return delta.y < 0 && delta.x == 0;
    //             case JPSDir.SE:
    //                 return delta.y < 0 && delta.x > 0 && Mathf.Abs(delta.x) == Mathf.Abs(delta.y);
    //             case JPSDir.SW:
    //                 return delta.y < 0 && delta.x < 0 && Mathf.Abs(delta.x) == Mathf.Abs(delta.y);
    //         }
    //         return false;
    //     }
    //     
    //     /// 计算差值
    //     private int DiffNode(Vector2Int x, Vector2Int y)
    //     {
    //         var diff = x - y;
    //         return Mathf.Max(Mathf.Abs(diff.x), Mathf.Abs(diff.y)); 
    //     }
    //     private int DiffNode_x(Vector2Int x, Vector2Int y)
    //     {
    //         var diff = x - y;
    //         return Mathf.Abs(diff.x);
    //     }
    //     private int DiffNode_y(Vector2Int x, Vector2Int y)
    //     {
    //         var diff = x - y;
    //         return Mathf.Abs(diff.y);
    //     }
    //
    //     /// <summary>
    //     /// 回溯生成路径
    //     /// </summary>
    //     /// <param name="node"></param>
    //     /// <returns></returns>
    //     private List<Vector2Int> Trace(JPSPlusNode node)
    //     {
    //         var path = new List<Vector2Int> { node.Location };
    //         while (node.Parent != null)
    //         {
    //             node = node.Parent;
    //             path.Add(node.Location);
    //         }
    //         path.Reverse();
    //         return path;
    //     }
    // }
}